#include "sm_core.h"
#include <stdint.h>
#include "tasks.h"

struct TaskComponents {
    uint16_t run;       // 运行标志
    uint16_t countdown; // 倒计时计数器（当前值）
    uint16_t period;    // 任务周期（固定值）
    uint16_t (*TaskHook)(uint8_t taskId, uint16_t events);
};

// #define MAX_TASKS 8
#define MAX_QUEUE_ITEMS 10 // 统一队列的最大项数
#define MAX_DATA_SIZE 20   // 每个数据项的最大大小

static struct TaskComponents  TaskComps[MAX_TASKS];
static uint8_t  task_count = 0;

// 就绪表位图（每个任务用1位表示）
#define BIT_MAP_MAX ((MAX_TASKS + 7) / 8)
static uint8_t  task_ready_table[BIT_MAP_MAX];       // 任务就绪位图
static uint8_t  task_data_ready_table[BIT_MAP_MAX];  // 数据就绪位图
static uint8_t  task_event_ready_table[BIT_MAP_MAX]; // 事件就绪位图

// 位操作宏定义
#define SET_TASK_READY(task_id) (task_ready_table[(task_id) >> 3] |= (1 << ((task_id)&0x07)))
#define CLR_TASK_READY(task_id) (task_ready_table[(task_id) >> 3] &= ~(1 << ((task_id)&0x07)))
#define IS_TASK_READY(task_id) (task_ready_table[(task_id) >> 3] & (1 << ((task_id)&0x07)))

#define SET_DATA_READY(task_id) (task_data_ready_table[(task_id) >> 3] |= (1 << ((task_id)&0x07)))
#define CLR_DATA_READY(task_id) (task_data_ready_table[(task_id) >> 3] &= ~(1 << ((task_id)&0x07)))
#define IS_DATA_READY(task_id) (task_data_ready_table[(task_id) >> 3] & (1 << ((task_id)&0x07)))

#define SET_EVENT_READY(task_id) (task_event_ready_table[(task_id) >> 3] |= (1 << ((task_id)&0x07)))
#define CLR_EVENT_READY(task_id) (task_event_ready_table[(task_id) >> 3] &= ~(1 << ((task_id)&0x07)))
#define IS_EVENT_READY(task_id) (task_event_ready_table[(task_id) >> 3] & (1 << ((task_id)&0x07)))

// 统一队列项类型
#define QUEUE_TYPE_EVENT 0
#define QUEUE_TYPE_DATA 1

// 统一队列项结构
typedef struct {
    uint8_t task_id;    // 目标任务ID
    uint8_t type;       // 类型：0=事件，1=数据
    uint8_t buffer_len; // 数据长度（事件时为0）
    union {
        uint16_t event;                // 事件值
        uint8_t buffer[MAX_DATA_SIZE]; // 数据内容
    } payload;
} UnifiedQueueItem;

// 统一队列（环形缓冲）
typedef struct {
    UnifiedQueueItem items[MAX_QUEUE_ITEMS];
    uint8_t head;
    uint8_t tail;
} UnifiedQueue;

static UnifiedQueue  unified_queue;

// 快速查找最高优先级就绪任务的函数
static uint8_t FindHighestReadyTask(uint8_t *ready_table)
{
    uint8_t i, j;
    uint8_t ready_byte;

    for (i = 0; i < BIT_MAP_MAX; i++) {
        ready_byte = ready_table[i];
        if (ready_byte != 0) {
            // 找到非零字节，查找最高位
            for (j = 0; j < 8; j++) {
                if (ready_byte & (1 << j)) {
                    uint8_t task_id = i * 8 + j;
                    if (task_id < MAX_TASKS) {
                        return task_id;
                    }
                }
            }
        }
    }
    return 0xFF; // 没有就绪任务
}

static void TASK_Init(void)
{
    uint8_t i;

    task_count = 0;
    // 初始化就绪表
    for (i = 0; i < BIT_MAP_MAX; i++) {
        task_ready_table[i] = 0;
        task_data_ready_table[i] = 0;
        task_event_ready_table[i] = 0;
    }
    // 初始化统一队列
    unified_queue.head = unified_queue.tail = 0;
    // 由外部调用TASK_Register注册各个任务
}

// 检查任务是否还有更多数据项
static uint8_t HasMoreDataForTask(uint8_t task_id)
{
    uint8_t temp_tail = unified_queue.tail;

    while (temp_tail != unified_queue.head) {
        if (unified_queue.items[temp_tail].task_id == task_id &&
            unified_queue.items[temp_tail].type == QUEUE_TYPE_DATA) {
            return 1; // 还有数据
        }
        temp_tail = (temp_tail + 1) % MAX_QUEUE_ITEMS;
    }
    return 0; // 没有更多数据
}

// 检查任务是否还有更多事件项
static uint8_t HasMoreEventForTask(uint8_t task_id)
{
    uint8_t temp_tail = unified_queue.tail;

    while (temp_tail != unified_queue.head) {
        if (unified_queue.items[temp_tail].task_id == task_id &&
            unified_queue.items[temp_tail].type == QUEUE_TYPE_EVENT) {
            return 1; // 还有事件
        }
        temp_tail = (temp_tail + 1) % MAX_QUEUE_ITEMS;
    }
    return 0; // 没有更多事件
}

// 发送数据到指定任务
TASK_RESULT TASK_SendData(uint8_t task_id, uint8_t *buf, uint8_t len)
{
    uint8_t i;
    uint8_t next;

    if (task_id >= task_count)
        return TASK_ERR_INVALID;
    if (len > MAX_DATA_SIZE)
        return TASK_ERR_FULL;

    next = (unified_queue.head + 1) % MAX_QUEUE_ITEMS;
    if (next == unified_queue.tail)
        return TASK_ERR_FULL; // 队列满

    // 添加数据项到队列
    unified_queue.items[unified_queue.head].task_id = task_id;
    unified_queue.items[unified_queue.head].type = QUEUE_TYPE_DATA;
    unified_queue.items[unified_queue.head].buffer_len = len;

    for (i = 0; i < len; ++i) {
        unified_queue.items[unified_queue.head].payload.buffer[i] = buf[i];
    }

    unified_queue.head = next;

    // 设置数据就绪标志
    SET_DATA_READY(task_id);
    SET_TASK_READY(task_id);

    return TASK_OK;
}

// 接收指定任务的数据，返回实际长度
uint8_t TASK_RecvData(uint8_t task_id, uint8_t *buf, uint8_t maxlen)
{
    uint8_t len = 0;
    uint8_t i;
    uint8_t temp_tail = unified_queue.tail;

    if (task_id >= task_count)
        return 0;

    // 快速检查是否有数据就绪
    if (!IS_DATA_READY(task_id))
        return 0;

    // 查找该任务的数据项
    while (temp_tail != unified_queue.head) {
        if (unified_queue.items[temp_tail].task_id == task_id &&
            unified_queue.items[temp_tail].type == QUEUE_TYPE_DATA) {
            // 找到该任务的数据
            len = unified_queue.items[temp_tail].buffer_len;
            if (len > maxlen)
                len = maxlen;

            // 复制数据
            for (i = 0; i < len; ++i) {
                buf[i] = unified_queue.items[temp_tail].payload.buffer[i];
            }

            // 移除这个数据项（通过移动tail指针）
            unified_queue.tail = (temp_tail + 1) % MAX_QUEUE_ITEMS;

            // 检查是否还有其他数据项
            if (!HasMoreDataForTask(task_id)) {
                CLR_DATA_READY(task_id);
                // 如果没有事件也没有数据，清除任务就绪标志
                if (!IS_EVENT_READY(task_id)) {
                    CLR_TASK_READY(task_id);
                }
            }
            return len;
        }
        temp_tail = (temp_tail + 1) % MAX_QUEUE_ITEMS;
    }

    return 0; // 没有找到该任务的数据
}

TASK_RESULT TASK_SendEvent(uint8_t task_id, uint16_t event)
{
    uint8_t next;

    if (task_id >= task_count)
        return TASK_ERR_INVALID;

    next = (unified_queue.head + 1) % MAX_QUEUE_ITEMS;
    if (next == unified_queue.tail)
        return TASK_ERR_FULL;

    // 添加事件项到队列
    unified_queue.items[unified_queue.head].task_id = task_id;
    unified_queue.items[unified_queue.head].type = QUEUE_TYPE_EVENT;
    unified_queue.items[unified_queue.head].buffer_len = 0;
    unified_queue.items[unified_queue.head].payload.event = event;

    unified_queue.head = next;

    // 设置事件就绪标志
    SET_EVENT_READY(task_id);
    SET_TASK_READY(task_id);

    return TASK_OK;
}

// 任务取事件（无事件返回0）
uint16_t TASK_RecvEvent(uint8_t task_id)
{
    uint16_t evt = 0;
    uint8_t temp_tail = unified_queue.tail;

    if (task_id >= task_count)
        return 0;

    // 快速检查是否有事件就绪
    if (!IS_EVENT_READY(task_id))
        return 0;

    // 查找该任务的事件项
    while (temp_tail != unified_queue.head) {
        if (unified_queue.items[temp_tail].task_id == task_id &&
            unified_queue.items[temp_tail].type == QUEUE_TYPE_EVENT) {
            // 找到该任务的事件
            evt = unified_queue.items[temp_tail].payload.event;

            // 移除这个事件项（通过移动tail指针）
            unified_queue.tail = (temp_tail + 1) % MAX_QUEUE_ITEMS;

            // 检查是否还有其他事件项
            if (!HasMoreEventForTask(task_id)) {
                CLR_EVENT_READY(task_id);
                // 如果没有事件也没有数据，清除任务就绪标志
                if (!IS_DATA_READY(task_id)) {
                    CLR_TASK_READY(task_id);
                }
            }
            return evt;
        }
        temp_tail = (temp_tail + 1) % MAX_QUEUE_ITEMS;
    }

    return 0; // 没有找到该任务的事件
}

void TASK_Tick(void)
{
    uint8_t i;

    for (i = 0; i < task_count; i++) {
        if (TaskComps[i].countdown > 0x00) {
            TaskComps[i].countdown--;
            if (TaskComps[i].countdown == 0x00) {
                TaskComps[i].countdown = TaskComps[i].period; // 重置倒计时
                TaskComps[i].run = 1;
                SET_TASK_READY(i); // 设置任务就绪标志
            }
        }
    }
}

uint8_t TASK_Register(TaskEventInit init, uint16_t (*TaskHook)(uint8_t, uint16_t), uint8_t task_id, uint16_t period)
{
    uint8_t ret = 0;

    // 检查任务ID是否有效
    if (task_id >= MAX_TASKS) {
        return 0xFF; // 无效的任务ID
    }

    // 检查是否已经注册过该任务ID
    if (task_id < task_count) {
        return 0xFE; // 任务ID已被使用
    }

    // 调用初始化函数（如果提供）
    if (init) {
        init(task_id);
    }

    // 注册任务
    if (task_id < MAX_TASKS) {
        TaskComps[task_id].run = 0;
        TaskComps[task_id].countdown = period; // 初始倒计时值设为0，等待设置周期
        TaskComps[task_id].period = period;    // 初始周期值设为0
        TaskComps[task_id].TaskHook = TaskHook;

        // 更新任务计数
        if (task_id >= task_count) {
            task_count = task_id + 1;
        }

        ret = 1; // 注册成功
    } else {
        ret = 0; // 注册失败
    }

    return ret;
}

void TASK_Run(void)
{
    uint8_t ready_task;

    TASK_Init();
    while (1) {
        // 使用就绪表快速查找就绪任务
        ready_task = FindHighestReadyTask(task_ready_table);

        if (ready_task != 0xFF) {
            // 处理就绪任务
            if (TaskComps[ready_task].run) {
                // 取事件传递给任务
                uint16_t evt = TASK_RecvEvent(ready_task);
                if (TaskComps[ready_task].TaskHook)
                    TaskComps[ready_task].TaskHook(ready_task, evt);
                TaskComps[ready_task].run = 0;
            }

            // 检查是否还有数据需要处理
            if (IS_DATA_READY(ready_task)) {
                // 这里可以添加数据处理逻辑
                // 或者让任务自己调用TASK_RecvData
            }
        }

        // 如果没有就绪任务，可以进入低功耗模式
        // 或者处理其他系统任务
    }
}
